#include <Arduino.h> 
#include <SimpleFOC.h>     //仅用了simplefoc的传感器部分

#include "lowpass_filter.h"
#include "pid.h"
#include "DengFOC.h" 
#include "SPI.h"
#include <driver/mcpwm.h>



#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
float voltage_power_supply;
float Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0;
#define _3PI_2 4.71238898038f
float zero_electric_angle=0;
int PP=1,DIR=1;




//低通滤波初始化
LowPassFilter M0_Vel_Flt = LowPassFilter(0.01); // Tf = 10ms   //M0速度环
//PID
//PIDController vel_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = voltage_power_supply/2};
//PIDController angle_loop_M0 = PIDController{.P = 2, .I = 0, .D = 0, .ramp = 100000, .limit = 100};

PIDController vel_loop_M0 = PIDController{2, 0,0, 100000,voltage_power_supply/2};
PIDController angle_loop_M0 = PIDController{2,0, 0,100000,100};

//AS5047P
MagneticSensorSPI sensor = MagneticSensorSPI(AS5047_SPI, HSPI_SS);
SPIClass SPI_2(HSPI);

//=================PID 设置函数=================
//速度PID
void DFOC_M0_SET_VEL_PID(float P,float I,float D,float ramp)   //M0角度环PID设置
{
  vel_loop_M0.P=P;
  vel_loop_M0.I=I;
  vel_loop_M0.D=D;
  vel_loop_M0.output_ramp=ramp;
}
//角度PID
void DFOC_M0_SET_ANGLE_PID(float P,float I,float D,float ramp)   //M0角度环PID设置
{
  angle_loop_M0.P=P;
  angle_loop_M0.I=I;
  angle_loop_M0.D=D;
  angle_loop_M0.output_ramp=ramp;
}


//M0速度PID接口
float DFOC_M0_VEL_PID(float error)   //M0速度环
{
   return vel_loop_M0(error);
   
}
//M0角度PID接口
float DFOC_M0_ANGLE_PID(float error)
{
  return angle_loop_M0(error);
}


//初始变量及函数定义
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
//宏定义实现的一个约束函数,用于限制一个值的范围。
//具体来说，该宏定义的名称为 _constrain，接受三个参数 amt、low 和 high，分别表示要限制的值、最小值和最大值。该宏定义的实现使用了三元运算符，根据 amt 是否小于 low 或大于 high，返回其中的最大或最小值，或者返回原值。
//换句话说，如果 amt 小于 low，则返回 low；如果 amt 大于 high，则返回 high；否则返回 amt。这样，_constrain(amt, low, high) 就会将 amt 约束在 [low, high] 的范围内。1


// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle)
{
  float a = fmod(angle, 2*PI);   //取余运算可以用于归一化，列出特殊值例子算便知
  return a >= 0 ? a : (a + 2*PI);  
  //三目运算符。格式：condition ? expr1 : expr2 
  //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。
  //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。
  //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。
}


// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc) {
  // 限制上限
  Ua = _constrain(Ua, 0.0f, voltage_power_supply);
  Ub = _constrain(Ub, 0.0f, voltage_power_supply);
  Uc = _constrain(Uc, 0.0f, voltage_power_supply);
  // 计算占空比
  // 限制占空比从0到1
  float dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
  float dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
  float dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );

  mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_GEN_A, dc_a*100.0); 
  mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_1, MCPWM_GEN_A, dc_b*100.0); 
  mcpwm_set_duty(MCPWM_UNIT_0, MCPWM_TIMER_2, MCPWM_GEN_A, dc_c*100.0);
}

void setTorque(float Uq,float angle_el)
 {
  sensor.update();  //更新 机械角度  圈数值  当前时间 
  Uq=_constrain(Uq,-(voltage_power_supply)/2.0,(voltage_power_supply)/2.0);
  float Ud=0;
  angle_el = _normalizeAngle(angle_el);
  // 帕克逆变换
  Ualpha =  -Uq*sin(angle_el); 
  Ubeta =   Uq*cos(angle_el); 

  // 克拉克逆变换
  Ua = Ualpha + voltage_power_supply/2;
  Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
  Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
  setPwm(Ua,Ub,Uc);
}


void DFOC_Vbus(float power_supply)
{
  voltage_power_supply=power_supply;

  pinMode(PIN_UH, OUTPUT);
  pinMode(PIN_UL, OUTPUT);
  pinMode(PIN_VH, OUTPUT);
  pinMode(PIN_VL, OUTPUT);
  pinMode(PIN_WH, OUTPUT);
  pinMode(PIN_WL, OUTPUT); 

  digitalWrite(PIN_UH, LOW);
  digitalWrite(PIN_UL, LOW);
  digitalWrite(PIN_VH, LOW);
  digitalWrite(PIN_VL, LOW);
  digitalWrite(PIN_WH, LOW);
  digitalWrite(PIN_WL, LOW);

  mcpwminit();
  Serial.println("完成PWM初始化设置");

  //AS5047P
  SPI_2.begin(HSPI_SCLK, HSPI_MISO, HSPI_MOSI, HSPI_SS); // SCLK, MISO, MOSI, SS
  // initialise magnetic sensor hardware
  sensor.init(&SPI_2);
  Serial.println("编码器加载完毕");

  //PID 加载
  vel_loop_M0 = PIDController{2, 0,0,100000,voltage_power_supply/2};
 }


float _electricalAngle(){
  return  _normalizeAngle((float)(DIR *  PP) * sensor.getMechanicalAngle()-zero_electric_angle);
}


void DFOC_alignSensor(int _PP,int _DIR)    //电角度校准  
{ 
  PP=_PP;
  DIR=_DIR;
  setTorque(voltage_power_supply/2, _3PI_2);  //起劲
  delay(3000);//等待电机转动到特定角度
  sensor.update();    //更新角度，方便下面电角度读取
  zero_electric_angle=_electricalAngle();
  setTorque(0, _3PI_2);  //松劲（解除校准）
  Serial.print("0 electric_angle:  ");
  Serial.println(zero_electric_angle);
}

float DFOC_M0_Angle()
{
  return DIR*sensor.getAngle();
}

//无滤波
//float DFOC_M0_Velocity()
//{
//  return DIR*S0.getVelocity();
//}

//有滤波
float DFOC_M0_Velocity()
{
  //获取速度数据并滤波
  float vel_M0_ori=sensor.getVelocity();
  float vel_M0_flit=M0_Vel_Flt(DIR*vel_M0_ori);
  return vel_M0_flit;   //考虑方向
}

//==============串口接收==============
float motor_target;
int commaPosition;
String serialReceiveUserCommand() {
  
  // a string to hold incoming data
  static String received_chars;
  
  String command = "";

  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the string buffer:
    received_chars += inChar;

    // end of user input
    if (inChar == '\n') {
      
      // execute the user command
      command = received_chars;

      commaPosition = command.indexOf('\n');//检测字符串中的逗号
      if(commaPosition != -1)//如果有逗号存在就向下执行
      {
          motor_target = command.substring(0,commaPosition).toDouble();            //电机角度
          Serial.println(motor_target);
      }
      // reset the command buffer 
      received_chars = "";
    }
  }
  return command;
}


float serial_motor_target()
{
  return motor_target;
}



void mcpwminit()
{ 
  mcpwm_unit_t  unit0  = MCPWM_UNIT_0;
  mcpwm_timer_t timer0 = MCPWM_TIMER_0;
  mcpwm_timer_t timer1 = MCPWM_TIMER_1;
  mcpwm_timer_t timer2 = MCPWM_TIMER_2;

  mcpwm_deadtime_type_t deadtime_type = MCPWM_ACTIVE_HIGH_COMPLIMENT_MODE; // 高电平有效互补

  mcpwm_config_t pwm_config = 
  {
    .frequency = 20000, // 频率为20kHz
    .cmpr_a = 70.0, // 占空比
    .cmpr_b = 30.0, // 实验发现，死区类型设置为高电平互补模式时该数无效
    .duty_mode = MCPWM_DUTY_MODE_0, //占空比计算方式为高电平时间
    .counter_mode = MCPWM_UP_COUNTER //计数模式为向上计数
  };

  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, PIN_UH); //绑定引脚
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0B, PIN_UL); //绑定引脚
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, PIN_VH); //绑定引脚
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1B, PIN_VL); //绑定引脚
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2A, PIN_WH); //绑定引脚
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM2B, PIN_WL); //绑定引脚  

  mcpwm_init(unit0, timer0, &pwm_config);
  mcpwm_init(unit0, timer1, &pwm_config);
  mcpwm_init(unit0, timer2, &pwm_config);

  mcpwm_deadtime_enable(unit0, timer0, deadtime_type, 10, 10); //死区方式配置
  mcpwm_deadtime_enable(unit0, timer1, deadtime_type, 10, 10); //死区方式配置
  mcpwm_deadtime_enable(unit0, timer2, deadtime_type, 10, 10); //死区方式配置

  mcpwm_start(unit0, timer0);
  mcpwm_start(unit0, timer1);
  mcpwm_start(unit0, timer2);

  mcpwm_set_duty(unit0, timer0, MCPWM_GEN_A, 30.0); 
  mcpwm_set_duty(unit0, timer1, MCPWM_GEN_A, 40.0); 
  mcpwm_set_duty(unit0, timer2, MCPWM_GEN_A, 50.0); 
 
}

void six_step_control()
{
//步骤一 100001 u>w
  digitalWrite(PIN_UH, HIGH);
  digitalWrite(PIN_UL, LOW);
  digitalWrite(PIN_VH, LOW);
  digitalWrite(PIN_VL, LOW);
  digitalWrite(PIN_WH, LOW);
  digitalWrite(PIN_WL, HIGH);
  vTaskDelay(5);

  //步骤二 100100 u>v
  digitalWrite(PIN_UH, HIGH);
  digitalWrite(PIN_UL, LOW);
  digitalWrite(PIN_VH, LOW);
  digitalWrite(PIN_VL, HIGH);
  digitalWrite(PIN_WH, LOW);
  digitalWrite(PIN_WL, LOW); 
  vTaskDelay(5);

  //步骤三 000110 w>v
  digitalWrite(PIN_UH, LOW);
  digitalWrite(PIN_UL, LOW);
  digitalWrite(PIN_VH, LOW);
  digitalWrite(PIN_VL, HIGH);
  digitalWrite(PIN_WH, HIGH);
  digitalWrite(PIN_WL, LOW); 
  vTaskDelay(5);

  //步骤四 010010 w>u
  digitalWrite(PIN_UH, LOW);
  digitalWrite(PIN_UL, HIGH);
  digitalWrite(PIN_VH, LOW);
  digitalWrite(PIN_VL, LOW);
  digitalWrite(PIN_WH, HIGH);
  digitalWrite(PIN_WL, LOW);
  vTaskDelay(5);

  //步骤五 011000 v>u
  digitalWrite(PIN_UH, LOW);
  digitalWrite(PIN_UL, HIGH);
  digitalWrite(PIN_VH, HIGH);
  digitalWrite(PIN_VL, LOW);
  digitalWrite(PIN_WH, LOW);
  digitalWrite(PIN_WL, LOW);
  vTaskDelay(5);

  //步骤六 001001 v>w
  digitalWrite(PIN_UH, LOW);
  digitalWrite(PIN_UL, LOW);
  digitalWrite(PIN_VH, HIGH);
  digitalWrite(PIN_VL, LOW);
  digitalWrite(PIN_WH, LOW);
  digitalWrite(PIN_WL, HIGH);
  vTaskDelay(5);

}



//================简易接口函数================
void DFOC_M0_set_Velocity_Angle(float Target)
{
 setTorque(DFOC_M0_VEL_PID(DFOC_M0_ANGLE_PID((Target-DFOC_M0_Angle())*180/PI)),_electricalAngle());   //角度闭环
}

void DFOC_M0_setVelocity(float Target)
{
  setTorque(DFOC_M0_VEL_PID((serial_motor_target()-DFOC_M0_Velocity())*180/PI),_electricalAngle());   //速度闭环
}

void DFOC_M0_set_Force_Angle(float Target)   //力位
{
  setTorque(DFOC_M0_ANGLE_PID((Target-DFOC_M0_Angle())*180/PI),_electricalAngle());
}

void DFOC_M0_setTorque(float Target)
{
  setTorque(Target,_electricalAngle());
}
